#include <stdlib.h>
#include <string.h>

#include "log.h"
#include "memmap.h"

#include "blkalloc.h"

void blk_add(BlkAlloc *alloc, u32 start, u32 size, u8 taken, u8 empty)
{
    if (alloc->blocks == NULL)
        alloc->blocks = malloc(sizeof(Block));
    else
        alloc->blocks = realloc(alloc->blocks, (alloc->numblocks + 1) * sizeof(Block));

    alloc->blocks[alloc->numblocks].start = start;
    alloc->blocks[alloc->numblocks].size = size;
    alloc->blocks[alloc->numblocks].taken = taken;
    strcpy(alloc->blocks[alloc->numblocks].tag, "Empty");
    alloc->numblocks++;
    if (empty)
    {
        u32 i;
        for (i = 0; i < size; i++)
            mem_write8(start + i, 0);
    }
}

void blk_del(BlkAlloc *alloc, u8 pos)
{
    u8 i;
    alloc->numblocks--;
    for (i = pos; i < alloc->numblocks; i++)
        alloc->blocks[i] = alloc->blocks[i + 1];
}

void blk_setTag(Block *blk, const char *tag)
{
    if (tag) {
        strncpy(blk->tag, tag, 15);
        blk->tag[strlen(tag)] = '\0';
    }
    else
        strcpy(blk->tag, "---");
}

void blkAlloc_sort(BlkAlloc *alloc)
{
    u8 i, j;
    u32 min = 0xffffffff;
    u8 minid = 0xff;
    Block *newblks = malloc(alloc->numblocks * sizeof(Block));

    for (i = 0; i < alloc->numblocks; i++)
    {
        for (j = 0; j < alloc->numblocks; j++)
        {
            if (alloc->blocks[j].start < min) {
                min = alloc->blocks[j].start;
                minid = j;
            }
        }
        newblks[i] = alloc->blocks[minid];
        alloc->blocks[minid].start = -1;
        min = 0xffffffff;
    }

    for (i = 0; i < alloc->numblocks; i++)
        alloc->blocks[i] = newblks[i];

    free(newblks);
}

void blkAlloc_init(BlkAlloc *alloc, u32 start, u32 size, char *name)
{
    alloc->numblocks = 0;
    alloc->blocks = NULL;
    alloc->name = malloc(sizeof(char) * (strlen(name) + 1));
    alloc->name[0] = '\0';
    alloc->start = start;
    alloc->end = start + size;
    strcpy(alloc->name, name);
    blk_add(alloc, start, size, 0, 0);
}

void blkAlloc_shutdown(BlkAlloc *alloc)
{
    if (alloc->blocks)
        free(alloc->blocks);
    free(alloc->name);
    alloc->numblocks = 0;
    alloc->blocks = NULL;
}

u32 blkAlloc_alloc(BlkAlloc *alloc, u32 size, u8 fromEnd, const char *tag, u8 empty)
{
    s32 i;
    size = (size + 3) & ~3;

    if (!fromEnd)
    {
        for (i = 0; i < alloc->numblocks; i++)
        {
            Block *b = &alloc->blocks[i];
            if (!b->taken && b->size >= size)
            {
                if (b->size == size)
                {
                    b->taken = 1;
                    blk_setTag(b, tag);
                    return b->start;
                }
                else
                {
                    u32 start;
                    blk_add(alloc, b->start + size, b->size - size, 0, empty);
                    b = &alloc->blocks[i];
                    b->taken = 1;
                    b->size = size;
                    blk_setTag(b, tag);
                    start = b->start;
                    blkAlloc_sort(alloc);
                    blkAlloc_listBlocks(alloc);
                    return start;
                }
            }
        }
    }
    else
    {
        for (i = alloc->numblocks - 1; i >= 0; i--)
        {
            Block *b = &alloc->blocks[i];
            if (!b->taken && b->size >= size)
            {
                if (b->size == size)
                {
                    b->taken = 1;
                    blk_setTag(b, tag);
                    return b->start;
                }
                else
                {
                    u32 start;
                    blk_add(alloc, b->start, b->size - size, 0, empty);
                    b = &alloc->blocks[i];
                    b->start += b->size - size;
                    b->taken = 1;
                    b->size = size;
                    blk_setTag(b, tag);
                    start = b->start;
                    blkAlloc_sort(alloc);
                    blkAlloc_listBlocks(alloc);
                    return start;
                }
            }
        }
    }

    _log(ERR, HLE, "Block Allocator failed to allocate %d (%08x) bytes of contiguous memory", size, size);
    return 0;
}

u32 blkAlloc_allocAt(BlkAlloc *alloc, u32 address, u32 size, const char *tag, u8 empty)
{
    u8 i;

    size = (size + 3) & ~3;

    for (i = 0; i < alloc->numblocks; i++)
    {
        Block *b = &alloc->blocks[i];
        if (b->start <= address && b->start + b->size > address)
        {
            if (b->taken) {
                _log(ERR, HLE, "Block allocator AllocAt failed, block taken! %08x, %d", address, size);
                return -1;
            }
            else
            {
                if (b->start == address)
                {
                    blk_add(alloc, b->start + size, b->size - size, 0, empty);
                    b = &alloc->blocks[i];
                    b->taken = 1;
                    b->size = size;
                    blk_setTag(b, tag);
                    blkAlloc_sort(alloc);
                    blkAlloc_listBlocks(alloc);
                    return address;
                }
                else
                {
                    blk_add(alloc, b->start, address - b->start, 0, empty);
                    b = &alloc->blocks[i];
                    if (b->start + b->size > address + size) {
                        blk_add(alloc, address + size, (b->start + b->size) - (address + size), 0, empty);
                        b = &alloc->blocks[i];
                    }
                    b->taken = 1;
                    b->start = address;
                    b->size = size;
                    blk_setTag(b, tag);
                    blkAlloc_sort(alloc);
                    blkAlloc_listBlocks(alloc);
                    return address;
                }
            }
        }
    }

    _log(ERR, HLE, "Block allocator AllocAt failed :( %08x, %d\n", address, size);
    return -1;
}

/* Merge free blocks */
void blkAlloc_merge(BlkAlloc *alloc)
{
    u8 found;
    u8 i, j;
    do
    {
        found = 0;
        for (i = 0, j = 1; j < alloc->numblocks && !found; i++, j++)
        {
            Block *b1 = &alloc->blocks[i];
            Block *b2 = &alloc->blocks[j];

            if (b1->taken == 0 && b2->taken == 0)
            {
                _log(INF, HLE, "Block Alloc found adjacent free blocks - merging");
                b1->size += b2->size;
                blk_del(alloc, j);
                found = 1;
            }
        }
    } while (found);
}

void blkAlloc_free(BlkAlloc *alloc, u32 position)
{
    Block *b = blkAlloc_getBlkFromAddr(alloc, position);
    if (b) {
        b->taken = 0;
        blkAlloc_merge(alloc);
    }
    else
        _log(ERR, HLE, "BlockAllocator : invalid free %08x", position);
}

Block *blkAlloc_getBlkFromAddr(BlkAlloc *alloc, u32 addr)
{
    u8 i;
    for (i = 0; i < alloc->numblocks; i++)
    {
        Block *b = &alloc->blocks[i];
        if (b->start <= addr && b->start + b->size > addr)
            return b;
    }
    return NULL;
}

/* Get the largest free block */
u32 blkAlloc_getMaxBlk(BlkAlloc *alloc)
{
    u32 max = 0;
    u8 i;
    for (i = 0; i < alloc->numblocks; i++)
    {
        Block *b = &alloc->blocks[i];
        if (!b->taken)
            if (b->size > max)
                max = b->size;
    }
    return max;
}

/* Get the total free size */
u32 blkAlloc_getFreeSize(BlkAlloc *alloc)
{
    u32 total = 0;
    u8 i;
    for (i = 0; i < alloc->numblocks; i++)
    {
        Block *b = &alloc->blocks[i];
        if (!b->taken)
            total += b->size;
    }
    return total;
}

u8 blkAlloc_addrInBlock(BlkAlloc *alloc, u32 address)
{
    return (address >= alloc->start && address < alloc->end);
}

void blkAlloc_listBlocks(BlkAlloc *alloc)
{
    u8 i;
    _log(INF, HLE, "--- Blocks in %s:", alloc->name);
    for (i = 0; i < alloc->numblocks; i++)
    {
        Block *b = &alloc->blocks[i];
        _log(INF, HLE, "Block: %08x - %08x, size %08x, %staken, tag %s", b->start, b->start + b->size, b->size, b->taken ? "":"not ", b->tag);
    }
    _log(INF, HLE, "---");
}

